// v1.0 (22/10/14): created to demonstrate velocity control of the robot, and in XYZ mode

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>

#include "MatWork.hpp"

#include "DENSOrobot.hpp"


using namespace cv;
using namespace std;

enum InputMode {READ_PENDANT, INV_KINE };


int run_RS232 = 1;  //0: NO; 1: YES
int mode = DENSOrobot::CTRL_MODE_XYZ;
InputMode ipm = READ_PENDANT;


int InitWithViaPoses(DENSOrobot& DSrb, int num)
{
	int ret = 1;
	Mat ViaPose;
	vector<Mat> Poses;

	// define destination pose in XYZ pose
	double DstPose_ar[6] = {150.0, 50.0, 480.0, 0, CV_PI/2, 0};
	ViaPose = Mat(6, 1, CV_64F, DstPose_ar);			// pose should be in 6x1 format
	Poses.push_back(ViaPose);
	
	DstPose_ar[0] = 160.0;
	DstPose_ar[1] = 50.0; 
	DstPose_ar[2] = 480.0; 
	DstPose_ar[3] = 0.0; 
	DstPose_ar[4] = CV_PI/2; 
	DstPose_ar[5] = 0.0; 
	Poses.push_back(ViaPose);

	DstPose_ar[0] = 170.0;
	DstPose_ar[1] = 50.0; 
	DstPose_ar[2] = 480.0; 
	DstPose_ar[3] = 0.0; 
	DstPose_ar[4] = CV_PI/2; 
	DstPose_ar[5] = 0.0; 
	Poses.push_back(ViaPose);
	
	Mat traj;

	DSrb.inputPoses(Poses, DENSOrobot::CTRL_MODE_XYZ);  

	// ret = DSrb.TrajPlanner(OrgPose, DstPose, traj, DENSOrobot::CTRL_MODE_XYZ);
	ret = DSrb.PresetMove(2);

	if (!ret)
	{
		printf("Preset move has error!\n");
	}

	return ret;
}


int main()
{
    int ret;
		
	// init robot with "move to ready pose" init mode
	DENSOrobot DSrb = DENSOrobot(1, DENSOrobot::OPER_MODE_XYZ, DENSOrobot::INIT_MOVE_TO_POSE);
	getchar();

	// prepare some poses for the robot and move to one via pose
	ret = InitWithViaPoses(DSrb, 3);

	printf("Current XYZ pose is:\n");
	MATmtxdisp(DSrb.CPose);
	//getchar();

	// Start to run Velocity control
	int count = 100;
	Mat VelScrew = Mat::zeros(6,1,CV_64FC1);
	while (count > 0)
	{
		count--;
		
		VelScrew.at<double>(0) = 0.1;
		VelScrew.at<double>(1) = 0.1;
		VelScrew.at<double>(2) = 0.1;
		VelScrew.at<double>(3) = 0.1;
		VelScrew.at<double>(4) = 0.1;
		VelScrew.at<double>(5) = 0.1;

		DSrb.VelCtrl(VelScrew, DENSOrobot::CTRL_MODE_XYZ);

		MATmtxdisp(DSrb.CPose);
	}

	getchar();
	
	return ret;
}
